// #define _GNU_SOURCE
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/mman.h>
#include <time.h>
#include <sched.h> /* sched_setscheduler() */
#include <pthread.h>
#include <dirent.h>
#include "ecrt.h"
#include "./tinyxml2.h"
#include "define.h"


using namespace tinyxml2;


/* 控制模式 */
#if ENABLE_PP_MODE
    #define OPERATION_MODE 1        // PP模式
    #define TARGET_POSITION 20    // 目标位置
    #define CONTOUR_VELOCITY 50000 // 轮廓速度
    #define CONTOUR_ACCELERATION 50000 // 轮廓加速度
    #define CONTOUR_DECELERATION 50000 // 轮廓减速度
    #define POSITION_FACTOR_NUMERATOR 1 // 位置因子分子
    #define POSITION_FACTOR_DENOMINATOR 1 // 位置因子分母
#elif ENABLE_PV_MODE
    #define OPERATION_MODE 3       // PV
    #define TARGET_VELOCITY 131072 // 目标速度
    #define MAX_VELOCITY 13107200  // 最大轮廓速度
#elif ENABLE_CSP_MODE
    #define OPERATION_MODE 8        // CSP
    #define TARGET_POSITION 20000    // 目标位置
#elif ENABLE_CSV_MODE
#endif


/* Time Parameters */
#define NSEC_PER_SEC (1000000000)                       // 定义基础纳秒单位（s/ns）
#define TASK_FREQUENCY          1000                    // 控制频率（HZ）
#define PERIOD_NS (NSEC_PER_SEC / TASK_FREQUENCY)       // 控制周期的纳秒表示（1000000 * 1ns = 1ms）


/*Time Test*/
struct timespec scheduler_start, scheduler_end;
struct timespec ethercat_scan_start, ethercat_scan_end;
struct timespec ethercat_scan_full_start, ethercat_scan_full_end;
struct timespec ethercat_check_status_start, ethercat_check_status_end;
uint64_t scheduler_time_ns, ethercat_scan_time_ns, ethercat_scan_full_time_ns, ethercat_check_status_time_ns;


/*EtherCAT*/
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};
static ec_slave_config_t *sc;
static ec_slave_config_state_t sc_state = {};

ec_slave_info_t ec_slave_info;


/*Process Data*/
static uint8_t *domain1_pd = NULL;

/* Master 0, Slave 0, "InoSV630N"
 * Vendor ID:       0x00100000
 * Product code:    0x000c0112
 * Revision number: 0x00010000
 */
#define SV630N_pos         0,0                        /*EtherCAT address on the bus*/
#define SV630N_VID_PID           0x00000100,0x00000100   /*Vendor ID, product code*/


/*Config PDOs*/
static ec_pdo_entry_info_t device_pdo_entries[] = {
    /*RxPdo 0x1600*/
    {0x6040, 0x00, 16}, // 控制字
    {0x6060, 0x00, 8 }, // 操作模式
#if ENABLE_PP_MODE
    {0x607A, 0x00, 32}, // 位置模式: 目标位置
    // {0x6081, 0x00, 32}, // 位置模式: 轮廓速度
    // {0x6083, 0x00, 32}, // 位置模式: 轮廓加速度
    // {0x6084, 0x00, 32}, // 位置模式: 轮廓减速度
    // {0x6093, 0x01, 32}, // 位置模式: 位置因子分子
    // {0x6093, 0x02, 32}, // 位置模式: 位置因子分母
#elif ENABLE_PV_MODE
    {0x60FF, 0x00, 32}, // 速度模式: 目标速度
    {0x607F, 0x00, 32}, // 速度模式: 最大轮廓速度
#elif ENABLE_CSP_MODE
    {0x607A, 0x00, 32}, // 位置模式: 目标位置
#elif ENABLE_CSV_MODE    
#endif
    /*TxPdo 0x1A00*/
    {0x6041, 0x00, 16}, // 状态字
    {0x606C, 0x00, 32}, // 实际速度
    {0x6064, 0x00, 32}  // 实际位置
};



static ec_pdo_info_t device_pdos[] = {

#if ENABLE_PP_MODE
    {0x1600, 3, device_pdo_entries + 0 },
    {0x1A00, 3, device_pdo_entries + 3 }
#elif ENABLE_PV_MODE
    {0x1600, 4, device_pdo_entries + 0 },
    {0x1A00, 3, device_pdo_entries + 4 }
#elif ENABLE_CSP_MODE
    {0x1600, 3, device_pdo_entries + 0 },
    {0x1A00, 3, device_pdo_entries + 3 }
#elif ENABLE_CSV_MODE    
#endif
};


static ec_sync_info_t device_syncs[] = {
    { 0, EC_DIR_OUTPUT, 0, NULL, EC_WD_ENABLE },
    { 1, EC_DIR_INPUT, 0, NULL, EC_WD_ENABLE },
    { 2, EC_DIR_OUTPUT, 1, device_pdos + 0, EC_WD_ENABLE }, // SM2: RxPDO
    { 3, EC_DIR_INPUT, 1, device_pdos + 1, EC_WD_ENABLE },  // SM3: TxPDO
    { 0xFF}
};


/*Offsets for PDO entries*/
static struct{
    unsigned int operation_mode;
    unsigned int control_word;
#if ENABLE_PP_MODE
    unsigned int target_position;   // 位置模式: 目标位置
    // unsigned int contour_velocity;  // 位置模式: 轮廓速度
    // unsigned int contour_acceleration;  // 位置模式: 轮廓加速度
    // unsigned int contour_deceleration;  // 位置模式: 轮廓减速度
    // unsigned int position_factor_numerator;  // 位置模式: 位置因子分子
    // unsigned int position_factor_denominator;  // 位置模式: 位置因子分母
#elif ENABLE_PV_MODE
    unsigned int target_velocity;   // 速度模式: 目标速度
    unsigned int max_velocity;      // 速度模式: 最大轮廓速度
#elif ENABLE_CSP_MODE
    unsigned int target_position;   // 位置模式: 目标位置
#elif ENABLE_CSV_MODE    
#endif

    unsigned int status_word;
    unsigned int current_velocity;
    unsigned int current_position;
}offset;





/*******************************************************************************/

void check_domain1_state(void)
{
    ec_domain_state_t ds;
    ecrt_domain_state(domain1, &ds);
    if(ds.working_counter != domain1_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if(ds.wc_state != domain1_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }
    domain1_state = ds;
}

void check_master_state(void)
{
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);
    if(ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if(ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if(ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }
    master_state = ms;
}

void check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    ecrt_slave_config_state(sc, &s);
    if(s.al_state != sc_state.al_state) {
        printf("slave: State 0x%08X.\n", s.al_state);
    }
    if(s.online != sc_state.online) {
        printf("slave: %s.\n", s.online ? "online" : "offline");
    }
    if(s.operational != sc_state.operational) {
        printf("slave: %soperational.\n", s.operational ? "" : "Not ");
    }
    sc_state = s;
}








void EtherCat_Init()
{
    /*
        初始化EterCAT协议栈
    */
    printf("Requesting master...\n");
    master = ecrt_request_master(0);    // 申请EtherCAT主站资源（文件操作）
    if(!master) {
        exit(EXIT_FAILURE);
    }
    domain1 = ecrt_master_create_domain(master);    // 创建数据交换域
    if(!domain1) {
        exit(EXIT_FAILURE);
    }

}


void EtherCat_XML_Init()
{
    /*
        从站读取 vendor_id, product_code, revision_number
    */
    ecrt_master_get_slave(master, 0, &ec_slave_info);
    printf("####IGH_DEBUG Vendor ID: 0x%08X\n", ec_slave_info.vendor_id);
    printf("####IGH_DEBUG Product Code: 0x%08X\n", ec_slave_info.product_code);
    printf("####IGH_DEBUG Revision No: 0x%08X\n", ec_slave_info.revision_number);

    /*
        定义XML设备信息结构体
    */
    typedef struct {
        char filename[128];
        unsigned int vendor_id;
        unsigned int product_code;
        unsigned int revision_number;
    } XMLDeviceInfo;
    
    XMLDeviceInfo xml_devices[128];
    int xml_count = 0;
    memset(xml_devices, 0, sizeof(xml_devices));

    /*
        打开XML配置目录
    */
    DIR *dir;
    struct dirent *ent;
    const char *xml_dir = "/root/xml_config/";
    
    if ((dir = opendir(xml_dir)) == NULL) {
        perror("Failed to open XML directory");
        return;
    }

    /*
        遍历目录中的XML文件
    */
    while ((ent = readdir(dir)) != NULL && xml_count < 128) {
        if (strstr(ent->d_name, ".xml") == NULL) {
            continue;  // 跳过非XML文件
        }
        
        char filepath[256];
        snprintf(filepath, sizeof(filepath), "%s/%s", xml_dir, ent->d_name);
        
        /*
            加载并解析XML文件
        */
        XMLDocument doc;
        if (doc.LoadFile(filepath) != XML_SUCCESS) {
            printf("Failed to load XML file: %s\n", filepath);
            continue;
        }
        
        XMLElement *root = doc.RootElement();
        if (!root) {
            printf("Invalid XML format in: %s\n", filepath);
            continue;
        }
        
        /*
            提取Vendor ID
        */
        unsigned int vendor_id = 0;
        XMLElement *vendor = root->FirstChildElement("Vendor");
        if (vendor) {
            XMLElement *id = vendor->FirstChildElement("Id");
            if (id && id->GetText()) {
                const char *vendor_str = id->GetText();
                // 处理 "#x00100000" 格式的十六进制字符串
                if (strncmp(vendor_str, "#x", 2) == 0) {
                    vendor_id = strtoul(vendor_str + 2, NULL, 16);
                } else {
                    vendor_id = strtoul(vendor_str, NULL, 16);
                }
            }
        }
        
        /*
            查找设备描述节点
        */
        XMLElement *descriptions = root->FirstChildElement("Descriptions");
        if (!descriptions) continue;
        
        XMLElement *devices = descriptions->FirstChildElement("Devices");
        if (!devices) continue;
        
        XMLElement *device = devices->FirstChildElement("Device");
        while (device && xml_count < 128) {
            XMLElement *type = device->FirstChildElement("Type");
            if (!type) {
                device = device->NextSiblingElement("Device");
                continue;
            }
            
            /*
                提取产品代码和修订号
            */
            const char *product_attr = type->Attribute("ProductCode");
            const char *revision_attr = type->Attribute("RevisionNo");
            
            if (product_attr && revision_attr) {
                /*
                    转换十六进制字符串为整数
                */
                unsigned int product_code = 0;
                unsigned int revision_no = 0;
                
                // 处理产品代码格式 "#x000C0112"
                if (strncmp(product_attr, "#x", 2) == 0) {
                    product_code = strtoul(product_attr + 2, NULL, 16);
                } else {
                    product_code = strtoul(product_attr, NULL, 16);
                }
                
                // 处理修订号格式 "#x00010000"
                if (strncmp(revision_attr, "#x", 2) == 0) {
                    revision_no = strtoul(revision_attr + 2, NULL, 16);
                } else {
                    revision_no = strtoul(revision_attr, NULL, 16);
                }
                
                /*
                    存储设备信息（包含Vendor ID）
                */
                strncpy(xml_devices[xml_count].filename, ent->d_name, 
                        sizeof(xml_devices[xml_count].filename) - 1);
                xml_devices[xml_count].vendor_id = vendor_id;
                xml_devices[xml_count].product_code = product_code;
                xml_devices[xml_count].revision_number = revision_no;
                xml_count++;
            }
            
            device = device->NextSiblingElement("Device");
        }
    }
    closedir(dir);
    
    /*
        匹配实际设备与XML配置（包含Vendor ID）
    */
    int matched = 0;
    for (int i = 0; i < xml_count; i++) {
        if (xml_devices[i].vendor_id == ec_slave_info.vendor_id &&
            xml_devices[i].product_code == ec_slave_info.product_code &&
            xml_devices[i].revision_number == ec_slave_info.revision_number) {
            printf("Matched device configuration: %s\n", xml_devices[i].filename);
            printf("  Vendor ID: 0x%08X\n", xml_devices[i].vendor_id);
            printf("  Product Code: 0x%08X\n", xml_devices[i].product_code);
            printf("  Revision No: 0x%08X\n", xml_devices[i].revision_number);
            matched = 1;
            break;
        }
    }
    
    if (!matched) {
        printf("Warning: No matching XML configuration found for device!\n");
        printf("  Actual Vendor ID: 0x%08X\n", ec_slave_info.vendor_id);
        printf("  Actual Product Code: 0x%08X\n", ec_slave_info.product_code);
        printf("  Actual Revision No: 0x%08X\n", ec_slave_info.revision_number);
    }
}








void EtherCat_ReScan()
{
    /*
        配置从站信息
        从站位置、厂商ID、产品ID、修订号
    */
    printf("####IGH_DEBUG Vendor ID: 0x%08X\n", ec_slave_info.vendor_id);
    printf("####IGH_DEBUG Product Code: 0x%08X\n", ec_slave_info.product_code);
    printf("####IGH_DEBUG Revision No: 0x%08X\n", ec_slave_info.revision_number);

    ec_pdo_entry_reg_t domain1_regs[] = {
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6040, 0, &offset.control_word},
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6060, 0, &offset.operation_mode },
#if ENABLE_PP_MODE
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x607A, 0, &offset.target_position},  // 目标位置
        // {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6081, 0, &offset.contour_velocity},  // 轮廓速度
        // {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6083, 0, &offset.contour_acceleration},  // 轮廓加速度
        // {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6084, 0, &offset.contour_deceleration},  // 轮廓减速度
        // {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6093, 1, &offset.position_factor_numerator},  // 位置因子分子
        // {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6093, 2, &offset.position_factor_denominator},  // 位置因子分母
#elif ENABLE_PV_MODE
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x60FF, 0, &offset.target_velocity},  // 目标速度
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x607F, 0, &offset.max_velocity},     // 最大轮廓速度
#elif ENABLE_CSP_MODE
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x607A, 0, &offset.target_position},  // 目标位置
#elif ENABLE_CSV_MODE
#endif
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6041, 0, &offset.status_word},
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x606C, 0, &offset.current_velocity},
        {SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code, 0x6064, 0, &offset.current_position},
        {}
    };

    if(!(sc = ecrt_master_slave_config(master, SV630N_pos, ec_slave_info.vendor_id, ec_slave_info.product_code))) {
        fprintf(stderr, "Failed to get slave configuration for slave!\n");
        exit(EXIT_FAILURE);
    }


    /*
        配置从站PDO（过程数据对象）映射，注册域内的PDO条目列表
    */
    printf("Configuring PDOs...\n");
    if(ecrt_slave_config_pdos(sc, EC_END, device_syncs)) {
        fprintf(stderr, "Failed to configure slave PDOs!\n");
        exit(EXIT_FAILURE);
    }else {
        printf("*Success to configuring slave PDOs*\n");
    }
    if(ecrt_domain_reg_pdo_entry_list(domain1, domain1_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        exit(EXIT_FAILURE);
    }

    /*
        配置分布式时钟 (DC)
        设置从站为DC同步模式，使用主站时钟
        0x0300是分配给从站的时钟偏移量（根据什么设置？）
        从站同步周期为 PERIOD_NS 1ms
    */
    printf("Configuring distributed clocks...\n");
    ecrt_slave_config_dc(sc, 0x0300, PERIOD_NS, 0, 0, 0);


    /*
        激活主站
    */
    printf("Activating master...\n");
    if(ecrt_master_activate(master)) {
        exit(EXIT_FAILURE);
    }else {
        printf("*Master activated*\n");
    }
    if(!(domain1_pd = ecrt_domain_data(domain1))) {
        exit(EXIT_FAILURE);
    }


    /******************** 清除报警操作 - 开始 ********************/
    printf("Attempting to clear slave faults...\n");
    
    // 步骤1: 发送故障复位命令 (0x0080)
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x0080); // 设置故障复位位
    
    // 执行一次完整的数据交换
    struct timespec ts;
    clock_gettime(CLOCK_MONOTONIC, &ts);
    uint64_t now = (uint64_t)ts.tv_sec * NSEC_PER_SEC + ts.tv_nsec;
    
    ecrt_master_application_time(master, now);
    ecrt_master_receive(master);
    ecrt_domain_process(domain1);
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
    
    // 等待复位命令生效
    usleep(100000);
    
    // 步骤2: 清除复位位 (0x0000)
    EC_WRITE_U16(domain1_pd + offset.control_word, 0x0000);
    
    // 再次执行数据交换
    clock_gettime(CLOCK_MONOTONIC, &ts);
    now = (uint64_t)ts.tv_sec * NSEC_PER_SEC + ts.tv_nsec;
    
    ecrt_master_application_time(master, now);
    ecrt_master_receive(master);
    ecrt_domain_process(domain1);
    ecrt_domain_queue(domain1);
    ecrt_master_send(master);
    
    printf("Fault reset sequence completed.\n");

    // EC_WRITE_S8(domain1_pd + offset.operation_mode, OPERATION_MODE);
    ecrt_slave_config_sdo8(sc, 0x6060, 0, OPERATION_MODE);
    printf(" ####IGH_DEBUG Setting operation mode: %s\n", (ENABLE_PP_MODE | ENABLE_CSP_MODE) ? "Position" : "Velocity");

    printf("*It's working now*\n");
}




















/*******************************************************************************/

void cyclic_task()
{
    static uint16_t command=0x004F;//用来帮助判断状态字的值
    uint16_t status = 0, last_status = 0;
    struct timespec ts;
    uint64_t now = 0;

    uint64_t scheduler_time_array[2][20480] = {0};

    while(1) {
        
        // DC模式: 主站将当前时间戳广播给所有从站
        clock_gettime(CLOCK_MONOTONIC, &ts);
        now = (uint64_t)ts.tv_sec * 1000000000LL + ts.tv_nsec;
        ecrt_master_application_time(master, now);

        /*
            Receive process data
        */
        ecrt_master_receive(master);
        ecrt_domain_process(domain1);


        /*
            Check Master/slaves Status
        */
        clock_gettime(CLOCK_MONOTONIC, &ethercat_check_status_start);
        /*Check process data state(optional)*/
        check_domain1_state();
        //Check for master state
        check_master_state();
        //Check for slave configuration state(s)
        check_slave_config_states();
        clock_gettime(CLOCK_MONOTONIC, &ethercat_check_status_end);


        /*
            Read state_word
        */
        status = EC_READ_U16(domain1_pd + offset.status_word);//读取状态字


        /*
            DS402 CANOpen over EtherCAT status machine
            ESM
        */
        //设置控制模式
        // printf(" ####IGH_DEBUG offset.current_position =  \n", offset.current_position);
        if( (status & command) == 0x0040 ) {
            // pre-op -> safe-op
            EC_WRITE_U16(domain1_pd + offset.control_word, 0x0006 );
            EC_WRITE_S8(domain1_pd + offset.operation_mode, OPERATION_MODE);
            printf(" ####IGH_DEBUG Setting operation mode: %s\n", (ENABLE_PP_MODE | ENABLE_CSP_MODE) ? "Position" : "Velocity");
            command = 0x006F;
        }else if( (status & command) == 0x0021) {
            EC_WRITE_U16(domain1_pd + offset.control_word, 0x0007 );
            command = 0x006F;
        } else if( (status & command) == 0x0023) {
            EC_WRITE_U16(domain1_pd + offset.control_word, 0x000f );
            command = 0x006F;
        } else if((status & 0x006F) == 0x0027) {
#if ENABLE_PP_MODE
            EC_WRITE_S32(domain1_pd + offset.target_position, TARGET_POSITION);
            // EC_WRITE_S32(domain1_pd + offset.contour_velocity, CONTOUR_VELOCITY);
            // EC_WRITE_S32(domain1_pd + offset.contour_acceleration, CONTOUR_ACCELERATION);
            // EC_WRITE_S32(domain1_pd + offset.contour_deceleration, CONTOUR_DECELERATION);
            // EC_WRITE_S32(domain1_pd + offset.position_factor_numerator, POSITION_FACTOR_NUMERATOR);
            // EC_WRITE_S32(domain1_pd + offset.position_factor_denominator, POSITION_FACTOR_DENOMINATOR);
#elif ENABLE_PV_MODE
            EC_WRITE_S32(domain1_pd + offset.target_velocity, TARGET_VELOCITY);
            EC_WRITE_S32(domain1_pd + offset.max_velocity, MAX_VELOCITY);
#elif ENABLE_CSP_MODE
            EC_WRITE_S32(domain1_pd + offset.target_position, TARGET_POSITION);
#elif ENABLE_CSV_MODE
#endif
        }

        if(status != last_status) {
            printf(" cyclic_task status = 0x%08x \n", status);
            
            static int turn_count = 1;
            if(((status & command) == 0x0027) && (turn_count == 1)) {
                printf(" \n\n into OP-mode cyclic_task status = 0x%08x \n\n", status);
                clock_gettime(CLOCK_MONOTONIC, &ethercat_scan_full_end);
                turn_count = 0;
            }
        }
        last_status = status;

        // DC: 同步参考时钟: 主站发送全局同步信号（SYNC0），对齐所有从站的时钟
        ecrt_master_sync_reference_clock(master);
        // DC: 同步从站时钟: 补偿传输延迟，确保所有从站时钟严格同步
        ecrt_master_sync_slave_clocks(master);

        /*
            Send process data
        */
        ecrt_domain_queue(domain1);
        ecrt_master_send(master);
        

        // scheduler time test
        clock_gettime(CLOCK_MONOTONIC, &scheduler_start);
        usleep(1000000/TASK_FREQUENCY);
        clock_gettime(CLOCK_MONOTONIC, &scheduler_end);


        /*
            耗时测试
        */

        // scheduler_time_ns = (scheduler_end.tv_sec - scheduler_start.tv_sec) * 1000000000LL +
        //                    (scheduler_end.tv_nsec - scheduler_start.tv_nsec);
        // ethercat_check_status_time_ns = (ethercat_check_status_end.tv_sec - ethercat_check_status_start.tv_sec) * 1000000000LL +
        //             (ethercat_check_status_end.tv_nsec - ethercat_check_status_start.tv_nsec);
        

        
        // static int i = 0;
        // static int j = 0;
        // static double scheduler_time_ms_every = 0;
        // static double ethercat_check_status_time = 0;

        // if(i <= TASK_FREQUENCY * 10) {
        //     scheduler_time_array[0][i] = scheduler_time_ns;
        //     scheduler_time_array[1][i] = ethercat_check_status_time_ns;
        // }else{
        //     for(j=0; j<=TASK_FREQUENCY*10; j++) {
        //         double scheduler_time_ms = scheduler_time_array[0][j] / 1000000.0;
        //         double ec_check_time_ms = scheduler_time_array[1][j] / 1000000.0;
        //         scheduler_time_ms_every += scheduler_time_ms;
        //         ethercat_check_status_time += ec_check_time_ms;
        //         // printf(" ==== igh_time_test: scheduler_time[0][%d] = %.6f ms\n", j, scheduler_time_ms);
        //         // printf(" ==== igh_time_test: ethercat_check_status_time[0][%d] = %.6f ms\n\n", j, ec_time_ms);                
        //     }

        //     ethercat_scan_time_ns = (ethercat_scan_end.tv_sec - ethercat_scan_start.tv_sec) * 1000000000LL +
        //                         (ethercat_scan_end.tv_nsec - ethercat_scan_start.tv_nsec);
        //     ethercat_scan_full_time_ns = (ethercat_scan_full_end.tv_sec - ethercat_scan_full_start.tv_sec) * 1000000000LL +
        //                         (ethercat_scan_full_end.tv_nsec - ethercat_scan_full_start.tv_nsec);
            
        //     printf("\n\n =============== TEST END ===============\n");
        //     printf(" ==== igh_time_test: ethercat_scan_time_ns          = %.6f \n", ethercat_scan_time_ns / 1000000.0);
        //     printf(" ==== igh_time_test: ethercat_scan_full_time_ns     = %.6f \n", ethercat_scan_full_time_ns / 1000000.0);
        //     printf(" ==== igh_time_test: scheduler_time_ms_every        = %.6f \n", scheduler_time_ms_every/(TASK_FREQUENCY*10));
        //     printf(" ==== igh_time_test: ethercat_check_status_time     = %.6f \n\n", ethercat_check_status_time/(TASK_FREQUENCY*10));

        //     sleep(1000);
        //     i = 0;
        //     j = 0;
        // }
        // i++;

    }

}













/****************************************************************************/

int main(int argc, char **argv)
{
    /*
        线程亲和cpu3
    */
    cpu_set_t mask;
    CPU_ZERO(&mask); // 清空 CPU 集合
    CPU_SET(3, &mask);

    pthread_t tid = pthread_self();
    int cpu_number = pthread_setaffinity_np(tid, sizeof(cpu_set_t), &mask);
    if (cpu_number != 0) {
        perror("pthread_setaffinity_np failed");
    }
    for (int i = 0; i < CPU_SETSIZE; i++) {
        if (CPU_ISSET(i, &mask)) {
            printf("####ThreadPRI_CPUaffinity thread is running on cpu %d\n", i);
        }
    }

    /*
        内存锁定
        MCL_CURRENT锁定当前内存，MCL_FUTURE锁定后续分配的内存
    */
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }

    /*
        线程设置，最高优先级，可抢占
    */
    struct sched_param param = {};
    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    pthread_t self = pthread_self();

    if((pthread_setschedparam(self, SCHED_FIFO, &param)) != 0) {
        perror("sched_setscheduler failed");
    }






    /*
        EtherCat 启动
    */

    EtherCat_Init();
    EtherCat_XML_Init();
    /*
        总线扫描(不包括ESM)
    */
    clock_gettime(CLOCK_MONOTONIC, &ethercat_scan_start);
    clock_gettime(CLOCK_MONOTONIC, &ethercat_scan_full_start);
    EtherCat_ReScan();
    clock_gettime(CLOCK_MONOTONIC, &ethercat_scan_end);


    /*
        Eethercat Status Machine
    */
    cyclic_task();

    return EXIT_SUCCESS;
}


